﻿#include "protocol/codec.h"
#include "utils/utils.h"


Decoder::Decoder(int len) : DataBuffer(len){
    //rx_length = 0;
    b_escape = false;
}

void Decoder::append_data(u8 *buff, int len) {
    for (u8 *buff_end = buff + len; buff < buff_end; buff++) {
        append_data(*buff);
    }
}
void Decoder::append_data(u8 data) {
    if (data == CH_START) {
        clear();
        b_escape = false;
    }else if (data == CH_END) {
        if (Data.size() > 2 && Data.size() < CODEC_MAX_RX_COUNT) {
            u16 *p_crc = reinterpret_cast<u16 *>(Data.data() + Data.size()-2);
            u16 crc2 =Utils::crc16_get((u8 *)Data.data(), Data.size()-2);
            if (*p_crc == crc2) {
                Data.remove(Data.size()-2, 2);
                emit on_frame(Data);
            }
            clear();
        }
    }else if (data == CH_ESC) {
        b_escape = true;
    }else {
        if (b_escape) {
            b_escape = false;
            switch (data) {
                case CH_ESC_START:
                    data = CH_START;
                    break;
                case CH_ESC_END:
                    data = CH_END;
                    break;
                case CH_ESC_ESC:
                    data = CH_ESC;
                    break;
                default:
                    data = 0xFF;
            }
        }
        if (Data.size() < CODEC_MAX_RX_COUNT) {
            Data.append(data);
        }else {
            clear();
        }
    }
}

Encoder::Encoder(int len):DataBuffer(len) {
    append_u8(CH_START);
    crc = 0;
}

void Encoder::AppendValue8(u8 data){
    crc = Utils::crc16_update(crc, &data, 1);
    switch (data) {
    case CH_START:
        append_u8(CH_ESC);
        data = CH_ESC_START;
        break;

    case CH_END:
        append_u8(CH_ESC);
        data = CH_ESC_END;
        break;

    case CH_ESC:
        append_u8(CH_ESC);
        data = CH_ESC_ESC;
        break;
    }
    append_u8(data);
}

void Encoder::AppendBytes(u8 *data, int len){
    for (int i = 0; i < len; i++) {
        AppendValue8(data[i]);
    }
}

void Encoder::AppendValue16(u16 data){
    u8 value[2];
    Utils::encode_u16(value, data);
    AppendBytes(value, sizeof(value));
}

void Encoder::AppendValue24(u32 data){
    u8 value[3];
    Utils::encode_u24(value, data);
    AppendBytes(value, sizeof(value));
}

void Encoder::AppendValue32(u32 data){
    u8 value[4];
    Utils::encode_u32(value, data);
    AppendBytes(value, sizeof(value));
}

void Encoder::AppendFloat(float data){
    u8 value[4];
    Utils::encode_float(value, data);
    AppendBytes(value, sizeof(value));
}

void Encoder::finish(void) {
    AppendValue16(crc);
    append_u8(CH_END);
}
